//
// Created by liangwenhao on 2024/4/6.
//

#include "locate_control.h"
#include "math.h"
#include "stdlib.h"
#include "time.h"

Motor LocateMotor;///can2 0x203
float Locate_CrossAngle = 0;
float Base_TargetAngle = Base_Length/2;
uint8_t Locate_Init_Timer = 0;
uint8_t Locate_Init_Flag = 2;

/* ----------------------- Remote Struct ----------------------------------- */
/**             1                                                       1
//      ++ s1   3                                               ++ s2   3
//              2                                                       2
//							++    1684                                ++
//					 + +  + +                              + +  + +
//					+       c3                            +        c1
//					+        +                            +        +
//				364	+++ c2 +++    1684                    +++ c0 +++
//							 +    364                                    +
*/

/*
s1 UP 手动模式

s1 MID 随机固定
    s2 MID->UP->MID 进行一次移动
    s2 MID->DOWN->MID 回中

s1 DOWN 随机移动
    s2 MID 停止
    s2 UP 持续移动
    s2 DOWN 回中

*/

void LocateMotor_Limit()
{
    if(Locate_CrossAngle < 0) Locate_CrossAngle = 0;
    if(Locate_CrossAngle > Base_Length) Locate_CrossAngle = Base_Length;
}


void Base_Moveto(float Target)
{

}


void LocateMotor_Control()
{
    if(RC_Ctl.Rc_Dog.rc_dog < 1)
    {
        /// 手动
        if(RC_Ctl.rc.s1 == RC_SW_UP)
        {
            if(fabsf(RC_Ctl.back_one.channel0) > 0.2){
                Locate_CrossAngle+=(float)RC_Ctl.back_one.channel0*400;
                LocateMotor_Limit();
            }
        }

        /// 随机固定
        if(RC_Ctl.rc.s1 == RC_SW_MID)
        {
            if(RC_Ctl.rc.s2 != RC_Ctl_last.rc.s2)
            {
                Base_TargetAngle = (rand()%100)*Base_Length/100.0f;
            }
        }

        /// 随机移动
        if(RC_Ctl.rc.s1 == RC_SW_DOWN)
        {
            if(fabsf(RC_Ctl.back_one.channel0) > 0.2){
                Locate_CrossAngle+=(float)RC_Ctl.back_one.channel0*400;
            }
        }
    }

}


